#include "CollisionManagement.h"

int CollisionManagement::iteratorCount = 3;

CollisionManagement::CollisionManagement()
{
    // generateQuadTree(CollisionEntityRectangle(Vector2(1000,1000)));
    mesh.data = generateMesh(Vector2(), 100, 100, 32);
    this->mesh.meshHeight = 100;
    this->mesh.meshWidth = 100;
    this->mesh.cellSize = 32;
    meshChange = true;
    navmesh.setBoundVertexs({Vector2(), {Static::mapSize.x, 0}, Static::mapSize, {0, Static::mapSize.y}});
    // std::vector<Vector2> ps;
    // ps.push_back(Vector2(300, 300));
    // ps.push_back(Vector2(500, 300));
    // ps.push_back(Vector2(500, 500));
    // ps.push_back(Vector2(300, 500));
    // navmesh.generate(ps);
    // navmesh.init(Vector2(20, 20), Vector2(1000, 1000));

    // navmesh.triangulate({{300,300},{400,300},{400,400},{300,400}});
}

CollisionManagement::~CollisionManagement()
{
    for (auto it = collisisons_list.begin(); it != collisisons_list.end();)
    {
        it = collisisons_list.erase(it);
    }
    if (qtree != nullptr)
    {
        delete qtree;
    }
    if (mesh.data != nullptr)
    {
        delete[] mesh.data;
    }
}

const std::list<Collision *> *CollisionManagement::updateCollisions()
{

    for (auto it = collisisons_list.begin(); it != collisisons_list.end();)
    {
        Collision *collision = *it;
        if (collision->solved)
        {
            delete collision;
            it = collisisons_list.erase(it);
        }
        else
        {
            ++it;
        }
    }
    for (auto it = collsionEntities_list.begin(); it != collsionEntities_list.end();)
    {
        CollisionEntity *entity = *it;
        if (entity->getParent() == nullptr)
        {
            if(entity->isFixed()){
                rebuildNavmesh();
            }
            it = collsionEntities_list.erase(it);
        }
        else
        {
            if (qtree != nullptr)
            {
                std::list<CollisionEntity *> *list = qtree->retrieve(entity);
                collide(entity, list);
                delete list;
            }
            else
            {
                collide(entity, &collsionEntities_list);
            }

            ++it;
        }
    }
    if (isRebuildNavmesh)
    {
        _rebuildNavmesh();
    }

    return &collisisons_list;
}

void CollisionManagement::add(CollisionEntity *entity)
{
    if (entity == nullptr)
    {
        return;
    }
    if(entity->isFixed()){
        rebuildNavmesh();
    }
    collsionEntities_list.push_back(entity);
}

bool CollisionManagement::collide(CollisionEntity *a, std::list<CollisionEntity *> *list)
{
    if (list == nullptr)
    {
        return false;
    }
    bool mark = false;
    for (auto it = list->begin(); it != list->end(); it++)
    {
        CollisionEntity *b = *it;
        if (a == b)
        {
            continue;
        }
        Vector2 inner = a->contact(b);
        if (inner.length2() > 0.1f)
        {
            Collision *collision = new Collision();
            collision->a = a;
            collision->b = b;
            collision->direct = inner;
            collision->solved = true;
            collisisons_list.push_back(collision);
            mark = true;
        }
    }
    return mark;
}

std::vector<Vector2> CollisionManagement::findPath(CollisionEntity *entity, Vector2 target) const
{
    std::vector<Vector2> res;
    // 暂时禁用
    return res;
    auto stack = getPath_astar(entity, target.x, target.y);
    while (!stack.empty())
    {
        res.push_back(stack.top());
        stack.pop();
    }
    res.push_back(target);
    return res;
}

int *CollisionManagement::generateMesh(Vector2 ul, int mesh_wsize, int mesh_hsize, int meshCell)
{
    int size = mesh_wsize * mesh_hsize;
    int *mesh = new int[size];
    for (int i = 0; i < size; i++)
    {
        int x = i % mesh_wsize;
        int y = i / mesh_wsize;
        Vector2 pos = ul + Vector2(meshCell * x, meshCell * y);
        mesh[i] = contactMesh(pos, meshCell);
    }
    return mesh;
}

bool CollisionManagement::contactMesh(Vector2 pos, int meshSize)
{
    for (auto it = collsionEntities_list.begin(); it != collsionEntities_list.end(); ++it)
    {
        CollisionEntity *entity = *it;
        const AABB *aabb = entity->getAABB();
        if (!entity->isFixed() || aabb->dr.x < pos.x || aabb->dr.y < pos.y || aabb->ul.x > pos.x + meshSize || aabb->ul.y > pos.y + meshSize)
        {
        }
        else
        {
            return true;
        }
    }
    return false;
}

const std::list<Collision *> *CollisionManagement::getCollisions()
{
    return &collisisons_list;
}

std::stack<std::pair<int, int>> CollisionManagement::getPath_astar(int *mesh, int meshWidth, int meshHeight, int startX, int startY, int targetX, int targetY)
{
    std::stack<std::pair<int, int>> s;
    return s;
}

std::stack<Vector2> CollisionManagement::getPath_astar(CollisionEntity *entity, int targetX, int targetY) const
{
    std::priority_queue<MeshNode *, std::vector<MeshNode *>, compare> que;
    std::stack<Vector2> res;
    int startX = entity->getPosition().x / mesh.cellSize;
    int startY = entity->getPosition().y / mesh.cellSize;
    targetX /= mesh.cellSize;
    targetY /= mesh.cellSize;
    int h = (so_abs(startX - targetX) + so_abs(startY - targetY)) * 10;
    que.push(new MeshNode(startX, startY, 0, h, nullptr));
    // std::vector<int> temp = getCollisionEntityMeshCellIndex(entity);
    // Log::log("target<%d,%d,%d>\t", targetX, targetY);
    // for (auto it : temp)
    // {
    //     mesh.data[it] = 0;
    // }
    int dir[8][2] = {
        {1, 0}, {0, 1}, {-1, 0}, {0, -1}, {1, 1}, {1, -1}, {-1, -1}, {-1, 1}};

    int gs[8] = {10, 10, 10, 10, 14, 14, 14, 14};
    int *data = mesh.data;
    while (!que.empty())
    {
        bool alive = false;
        auto cur = que.top();
        que.pop();
        if (cur->x == targetX && cur->y == targetY)
        {
            // Log::log("find\n");
            // int i = 0;
            while (cur != nullptr)
            {
                // Log::log("%d ", i++);
                // Log::log("<%d,%d,%d>\t", cur->x, cur->y, cur->cost);
                MeshNode *temp = cur->parent;
                res.push(Vector2(cur->x * mesh.cellSize + mesh.cellSize / 2, cur->y * mesh.cellSize + mesh.cellSize / 2));
                delete cur;
                cur = cur->parent;
            }
            res.pop();
            return res;
        }

        for (int i = 0; i < 4; i++)
        {
            int x = cur->x + dir[i][0];
            int y = cur->y + dir[i][1];
            int index = y * mesh.meshWidth + x;
            if (x < 0 || x >= mesh.meshWidth || y < 0 || y >= mesh.meshHeight || data[index] != 0)
            {
                continue;
            }
            h = (so_abs(x - targetX) + so_abs(y - targetY)) * 10;
            que.push(new MeshNode(x, y, cur->g + gs[i], h, cur));
            data[index] = 2;
            alive = true;
        }

        if (!alive)
        {
            delete cur;
        }
    }
    for (int i = 0; i < mesh.meshHeight * mesh.meshWidth; i++)
    {
        if (data[i] == 2)
        {
            data[i] = 0;
        }
    }
    // for (auto it : temp)
    // {
    //     mesh.data[it] = 1;
    // }
    return res;
}

void CollisionManagement::generateQuadTree(const CollisionEntityRectangle &rect, int objects_max, int deep_max)
{
    if (qtree != nullptr)
    {
        delete qtree;
    }
    qtree = new QuadTree(rect, 0, objects_max, deep_max);
    // SDL_Log("generateQuadTree");
}

void CollisionManagement::updateQuadTree()
{
    if (qtree == nullptr)
    {
        return;
    }
    qtree->clear();
    for (auto it = collsionEntities_list.begin(); it != collsionEntities_list.end(); ++it)
    {
        qtree->insert(*it);
    }
}

QuadTree *CollisionManagement::getQuadTree() const
{
    return qtree;
}

void CollisionManagement::freeQuadTree()
{
    if (qtree != nullptr)
    {
        delete qtree;
    }
    qtree = nullptr;
}

const std::list<CollisionEntity *> *CollisionManagement::getCollisionEntities() const
{
    return &collsionEntities_list;
}

const Mesh2D *CollisionManagement::getMesh() const
{
    return &mesh;
}

void CollisionManagement::updateMesh()
{
    if (!meshChange)
    {
        return;
    }
    int size = mesh.meshHeight * mesh.meshWidth;
    for (int i = 0; i < size; i++)
    {
        int x = i % mesh.meshWidth;
        int y = i / mesh.meshWidth;
        Vector2 pos = Vector2(mesh.cellSize * x, mesh.cellSize * y);
        mesh.data[i] = contactMesh(pos, mesh.cellSize);
    }
    meshChange = false;
}

std::vector<int> CollisionManagement::getCollisionEntityMeshCellIndex(CollisionEntity *entity) const
{
    const AABB *aabb = entity->getAABB();
    Vector2 ul = aabb->ul;
    Vector2 dr = aabb->dr;
    ul = ul / mesh.cellSize;
    dr = dr / mesh.cellSize;
    std::vector<int> res;
    for (int i = ul.y; i < dr.y; ++i)
    {
        if (i < 0 || i >= mesh.meshHeight)
        {
            continue;
        }
        for (int j = ul.x; j < dr.x; ++j)
        {
            if (j < 0 || j >= mesh.meshWidth)
            {
                continue;
            }
            res.push_back(i * mesh.meshWidth + j);
        }
    }
    return res;
}

Navmesh *CollisionManagement::getNavmesh()
{
    return &navmesh;
}

void CollisionManagement::_rebuildNavmesh()
{
    navmesh.clear();
    for (auto it = collsionEntities_list.begin(); it != collsionEntities_list.end(); ++it)
    {
        CollisionEntity *entity = *it;
        if (entity->isFixed())
        {
            const AABB *aabb = entity->getAABB();
            Vector2 v1 = aabb->ul;
            Vector2 v2 = Vector2(aabb->dr.x, aabb->ul.y);
            Vector2 v3 = aabb->dr;
            Vector2 v4 = Vector2(aabb->ul.x, aabb->dr.y);
            std::vector<Vector2> ps = {v1, v2, v3, v4};
            for (int i = 0; i < 4; ++i)
            {
                navmesh.addRestrainEdge({ps[i], ps[(i + 1) % 4]});
            }
        }
    }
    navmesh.generate();
    isRebuildNavmesh = false;
}

const std::vector<Edge *> *CollisionManagement::getEdge() const
{
    return navmesh.getEdge();
}

void CollisionManagement::rebuildNavmesh()
{
    isRebuildNavmesh = true;
}
